package com.google.code.yargon.level;

import org.apache.log4j.Logger;

import com.google.code.yargon.engine.Cast;
import com.google.code.yargon.util.LogUtil;

import static com.google.code.yargon.util.Constants.*;


public class RouteFinder {
	
	private Route route;
	private Area area;
	private int maxStraight;
	
	private Logger log = LogUtil.getInstance();
	
	/**
	 * New RouteFinder for Route in Area
	 * @param route
	 * @param area
	 */
	public RouteFinder(Route route, Area area) {
		this.route = route;
		this.area = area;
		this.maxStraight = 10;
	}
	
	/**
	 * Set maximum allowed straight steps
	 * 
	 * @param maxStraight
	 */
	public void setMaxStraight(int maxStraight) {
		this.maxStraight = maxStraight;
	}
	
	/**
	 * Get maximum allowed straight steps
	 * 
	 * @return
	 */
	public int getMaxStraight() {
		return this.maxStraight;
	}
		
	/**
	 * Find a path from route source location to the destination. "Homing" algorithm.
	 * 
	 * @return
	 */
	public int solveHomingPath() {
		
		// Current travel & inspection locations, starting with route source point
		Location current = route.getSource();
		Location inspect = route.getSource();
		
		// Current direction, starting with random
		Direction dir = Direction.getRandomDirection();
			
		// Starting way point
		route.push(current);
			
		int nLoops = 0; 
		while(true) {
			
			int connectionType = checkConnection(dir, current);
			if(connectionType > 0) {
				route.solve();
				return connectionType;
			}
			
			// Loop control if route ends up impossible to be found
			nLoops++;
			if(nLoops > 100) { 
				return 0; 
			}
			
			// Choose a new direction
			Direction[] dl = selectDirections(current, dir);
			dir = selectDirection(dl, current);
					
			// Randomize how many steps to go
			int steps = Cast.castValueBetween(1, maxStraight);	
			
			log.trace("solve(): Current location:  "+current.toString()+" direction: "+dir.toString()+" steps: "+steps);
			if(dir.getDirection() == UNDEFINED) {
				log.warn("Could not determine next direction!");
				return 0;
			}
			
			switch(dir.getDirection()) {		
			case NORTH:
				for(int y = current.getY() + 1; y < current.getY() + steps + 1; y++) {			// Go in the defined direction max steps
					inspect = new Location(current.getX(), y, 0);
					if(area.isFree(inspect)) {													// Free block
						route.push(inspect);													// Add it to the route
						if(y == route.getDestination().getY()) {
							log.trace("solve(): Reached destination x-axis");
							break;
						}
						continue;
					} 
					else {																		// Non-free block
						connectionType = checkConnection(dir, current);
						if(connectionType > 0) {
							route.solve();
							return connectionType;
						} 
						log.trace("solve(): Obstacle at "+inspect.toString());
						break;
					}
				}
				break;
				
			case SOUTH:
				for(int y = current.getY() - 1; y > current.getY() - steps - 1; y--) {			// Go in the defined direction max steps
					inspect = new Location(current.getX(), y, 0);
					if(area.isFree(inspect)) {													// Free block
						route.push(inspect);													// Add it to the route
						if(y == route.getDestination().getY()) {
							log.trace("solve(): Reached destination x-axis");
							break;
						}

						continue;
					} 
					else {																		// Non-free block
						connectionType = checkConnection(dir, current);
						if(connectionType > 0) {
							route.solve();
							return connectionType;
						} 
						log.trace("solve(): Obstacle at "+inspect.toString());
						break;
					}
				}
				break;
				
			case EAST:
				for(int x = current.getX() + 1; x < current.getX() + steps + 1; x++) {			// Go in the defined cdection max steps
					inspect = new Location(x, current.getY(), 0);
					if(area.isFree(inspect)) {													// Free block
						route.push(inspect);													// Add it to the route
						if(x == route.getDestination().getX()) {
							log.trace("solve(): Reached destination y-axis");
							break;
						}
						continue;
					} 
					else {																		// Non-free block
						connectionType = checkConnection(dir, current);
						if(connectionType > 0) {
							route.solve();
							return connectionType;
						} 
						log.trace("solve(): Obstacle at "+inspect.toString());
						break;
					}
				}
				break;
				
			case WEST:
				for(int x = current.getX() - 1; x > current.getX() - steps - 1; x--) {			// Go in the defined cdection max steps
					inspect = new Location(x, current.getY(), 0);
					if(area.isFree(inspect)) {													// Free block
						route.push(inspect);													// Add it to the route
						if(x == route.getDestination().getX()) {
							log.trace("solve(): Reached destination y-axis");
							break;
						}
						continue;
					} 
					else {																		// Non-free block
						connectionType = checkConnection(dir, current);
						if(connectionType  > 0) {
							route.solve();
							return connectionType;
						} 
						log.trace("solve(): Obstacle at "+inspect.toString());
						break;
					}
				}
				break;
			}		
			current = route.getLast();
		}			
	}

	/*
	private Route findPath(Location src, Location dest, LevelArea level) {
		
		Route route = new Route();
	
		route.push(src);
		
		int n = 0;
		while(true) {

			List<Location> adjacents = new LinkedList<Location>();
			
			// Check 4 adjacent blocks
			Location north = new Location(curX, curY + 1, curLoc.getZ() + 1);
			Location east = new Location(curX + 1, curY, curLoc.getZ() + 1);
			Location south = new Location(curX, curY - 1, curLoc.getZ() + 1);
			Location west = new Location(curX - 1, curY, curLoc.getZ() + 1);
					
			// TODO: Could be done in one loop
			if(level.isFree(north)) {
				if(route.hasWayPoint(north) && route.getWayPoint(north).getZ() <= north.getZ()) {
					// Discard
				} else {
					adjacents.add(north);
				}
			}
			if(level.isFree(east)) {
				if(route.hasWayPoint(east) && route.getWayPoint(east).getZ() <= east.getZ()) {
					// Discard
				} else {
					adjacents.add(east);
				}
			}
			if(level.isFree(south)) {
				if(route.hasWayPoint(south) && route.getWayPoint(south).getZ() <= south.getZ()) {
					// Discard
				} else {
					adjacents.add(south);
				}
			}
			if(level.isFree(west)) {
				if(route.hasWayPoint(west) && route.getWayPoint(west).getZ() <= west.getZ()) {
					// Discard
				} else {
					adjacents.add(west);
				}
			}
	
			for(Location l : adjacents) {
				route.addWayPoint(l);
			}
		}	
			
		return route;
	} */
	
	/**
	 * Checks connection<br><br>
	 * Connection is accepted if it is the destination, or another already existing and connectable TunnelBlock exists nearby<br><br>
	 * Returns 1 if the connection was to destination, 2 if the connection was to another tunnel or 0 if no connection was found
	 * 
	 * @param suggested
	 * @param destination
	 * @param level
	 * @return
	 */
	private int checkConnection(Direction d, Location l) {
		
		Location e = l;
		
		switch(d.getDirection()) {
		case NORTH: e.toNorth(); break;
		case EAST:  e.toEast();  break;
		case SOUTH: e.toSouth(); break;
		case WEST:  e.toWest();  break;
		}
		
		// Next in direction is destination
		if(e.equals(route.getDestination())) {
			log.trace("acceptEndpoint(): Destination reached");
			return 1;
		} 
		
		// Any adjacent (not diagonal) block is a tunnel
		else if(area.isTunnel(l.toNorth()) || area.isTunnel(l.toEast()) || area.isTunnel(l.toSouth()) || area.isTunnel(l.toWest())) {
			log.trace("acceptEndpoint(): Tunnel connection reached");
			return 2;
		}
		return 0;
	}
	
	/**
	 * Choose new directions in preferred order
	 * 
	 * @return
	 */
	private Direction[] selectDirections(Location cl, Direction cd) {
		
		// New direction candidates in preferred order
		Direction[] dc = new Direction[4];
	
		if(cl.getX() == route.getDestination().getX()) {									// Destination x-axis
			if(cl.getY() < route.getDestination().getY()) {
				dc[0] = new Direction(NORTH);												// Primary choice towards target
				if(Cast.castValueBetween(1, 2) == 1) {										// Secondary and tertiary choices perpendicular randomly and last choice is going back
					dc[1] = new Direction(EAST);
					dc[2] = new Direction(WEST);
					dc[3] = new Direction(SOUTH);
				} else {
					dc[1] = new Direction(WEST);
					dc[2] = new Direction(EAST);
					dc[3] = new Direction(SOUTH);
				}
			} else if(cl.getY() > route.getDestination().getY()) {
				dc[0] = new Direction(SOUTH);												// Primary choice towards target
				if(Cast.castValueBetween(1, 2) == 1) {										// Secondary and tertiary choices perpendicular randomly and last choice is going back
					dc[1] = new Direction(EAST);
					dc[2] = new Direction(WEST);
					dc[3] = new Direction(NORTH);
				} else {
					dc[1] = new Direction(WEST);
					dc[2] = new Direction(EAST);
					dc[3] = new Direction(NORTH);
				}
			} else {
				log.warn("selectDirections(): Looks like we're at the destination but it was not detected earlier!");
				return null;
			}
		} 
		
		else if(cl.getY() == route.getDestination().getY()) {								// Destination y-axis
			if(cl.getX() < route.getDestination().getX()) {
				dc[0] = new Direction(EAST);												// Primary choice towards target
				if(Cast.castValueBetween(1, 2) == 1) {										// Secondary and tertiary choices perpendicular randomly and last choice is going back
					dc[1] = new Direction(NORTH);
					dc[2] = new Direction(SOUTH);
					dc[3] = new Direction(WEST);
				} else {
					dc[1] = new Direction(SOUTH);
					dc[2] = new Direction(NORTH);
					dc[3] = new Direction(WEST);
				}
			} else if(cl.getX() > route.getDestination().getX()) {
				dc[0] = new Direction(WEST);												// Primary choice towards target
				if(Cast.castValueBetween(1, 2) == 1) {										// Secondary and tertiary choices perpendicular randomly and last choice is going back
					dc[1] = new Direction(NORTH);
					dc[2] = new Direction(SOUTH);
					dc[3] = new Direction(EAST);
				} else {
					dc[1] = new Direction(SOUTH);
					dc[2] = new Direction(NORTH);
					dc[3] = new Direction(EAST);
				}
			} else {
				log.warn("selectDirections(): Looks like we're at the destination but it was not detected earlier!");
				return null;
			}
		}
		
		else if(cd.getDirection() == NORTH || cd.getDirection() == SOUTH) {					// Traveling vertically
			if(route.getDestination().getX() > cl.getX()) {									// Destination is to east, prefer east, then continue, then west, then back
				dc[0] = new Direction(EAST);														
				if(cd.getDirection() == NORTH) {
					dc[1] = new Direction(NORTH);
					dc[2] = new Direction(WEST);
					dc[3] = new Direction(SOUTH);
				} else {
					dc[1] = new Direction(SOUTH);
					dc[2] = new Direction(WEST);
					dc[3] = new Direction(NORTH);
				}
			} else if(route.getDestination().getX() < cl.getX())	{						// Destination is to west, prefer west, then continue, then east, then back
				dc[0] = new Direction(WEST);				
				if(cd.getDirection() == NORTH) {
					dc[1] = new Direction(NORTH);
					dc[2] = new Direction(EAST);
					dc[3] = new Direction(SOUTH);
				} else {
					dc[1] = new Direction(SOUTH);
					dc[2] = new Direction(EAST);
					dc[3] = new Direction(NORTH);
				}
			} else {
				log.warn("selectDirections(): Looks like we're at the destination y-axis but it was not detected earlier!");
			}
		} 
		
		else if(cd.getDirection() == EAST || cd.getDirection() == WEST) {					// Traveling horizontally
			if(route.getDestination().getY() > cl.getY()) {									// Destination is to north, prefer north, then continue, then south, then back
				dc[0] = new Direction(NORTH);				
				if(cd.getDirection() == EAST) {
					dc[1] = new Direction(EAST);
					dc[2] = new Direction(SOUTH);
					dc[3] = new Direction(WEST);
				} else {
					dc[1] = new Direction(WEST);
					dc[2] = new Direction(SOUTH);
					dc[3] = new Direction(EAST);
				}
			} else if(route.getDestination().getY() < cl.getY())	{						// Destination is to south, prefer south, then continue, then north, then back
				dc[0] = new Direction(SOUTH);				
				if(cd.getDirection() == EAST) {
					dc[1] = new Direction(EAST);
					dc[2] = new Direction(NORTH);
					dc[3] = new Direction(WEST);
				} else {
					dc[1] = new Direction(WEST);
					dc[2] = new Direction(NORTH);
					dc[3] = new Direction(EAST);
				}
			} else {
				log.warn("selectDirections(): Looks like we're at the destination y-axis but it was not detected earlier!");
			}	
		} else {
			log.warn("selectDirections(): Failed to determine new direction");
			return null;
		}
		for(int i = 0; i < dc.length; i++) {
			log.trace("selectDirections(): ["+i+"]: "+dc[i].toString());
		}
		return dc;
	}
	
	/**
	 * Select a free direction among the choices and return it, or return an undefined direction if the goal is met. 
	 * 
	 * Returns null if no acceptable directions were found.
	 * 
	 * @param dc
	 * @param l
	 * @return
	 */
	private Direction selectDirection(Direction[] dc, Location l) {
		
		Direction d = new Direction(UNDEFINED);
		
		for(int i = 0; i < dc.length; i++) {
			if(acceptDirection(dc[i], l)) {
				d = dc[i];
				break;
			} 
			/*
			else if(acceptEndpoint(dc[i], l)) {
				break;
			} */
		}
		log.trace("selectDirection(): "+d.toString()+" selected");
		return d;
	}
	
	/**
	 * Returns true if the next block from the given location to the given direction is free
	 * 
	 * @param d
	 * @param c
	 * @return
	 */
	private boolean acceptDirection(Direction d, Location c) {
		
		Location l = null;
		
		switch(d.getDirection()) {
		case NORTH: l = c.toNorth(); break;
		case EAST:  l = c.toEast();  break;
		case SOUTH: l = c.toSouth(); break;
		case WEST:  l = c.toWest();  break;
		}
		
		if(area.isFree(l)) {
			log.trace("acceptDirection(): "+d.toString()+" is free");
			return true;
		}	
		log.trace("acceptDirection(): "+d.toString()+" is blocked");
		return false;
	}
	
	/**
	 * Solve straight line between source and destination locations using Bresenham's line algorithm
	 * 
	 */
	public void solveLinePath() {
		
		// Deltas
		int dx = Math.abs(route.getDestination().getX() - route.getSource().getX());          	
		int dy = Math.abs(route.getDestination().getY() - route.getSource().getY());   
		
		// Initial point location at source
		int x = route.getSource().getX();                                 		
		int y = route.getSource().getY();   
		
		// Increments
		int ix1;
		int ix2;
		int iy1;
		int iy2;
		
		int den;
		int num;
		int numAdd;
		int numPts;
		
		if(route.getDestination().getX() >= route.getSource().getX()) {		// x-values are increasing
			ix1 = 1;
			ix2 = 1;
		}
		else {                                              				// .. or decreasing
			ix1 = -1;
			ix2 = -1;
		}

		if(route.getDestination().getY() >= route.getSource().getY()) {		// y-values are increasing
			iy1 = 1;
			iy2 = 1;
		}
		else {                                            					// .. or decreasing
			iy1 = -1;
			iy2 = -1;
		}

		if(dx >= dy) {
			ix1 = 0;                            
			iy2 = 0;                              
			den = dx;
			num = dx / 2;
			numAdd = dy;
			numPts = dx;           
		}
		else {                                            
			ix2 = 0;                             
			iy1 = 0;                              
			den = dy;
			num = dy / 2;
			numAdd = dx;
			numPts = dy;           
		}

		for(int i = 0; i <= numPts; i++) {
			route.push(x, y);
			
			num += numAdd; 
			if (num >= den) {
				num -= den;                    
		        x += ix1;                   
		        y += iy1;                   
			}
			x += ix2;                       
			y += iy2;                        
		}
		
		// Assume success
		route.solve();
	}
}
